#include "cartographer/rcm/imu/preintegrate.h"

namespace cartographer
{
    int RunImuPreintegrate(ImuPreintegrate::Ptr &integrator,
                           const std::deque<sensor::ImuData> imu_data,
                           const common::Time &time_start,
                           const common::Time &time_end)
    {
        std::vector<sensor::ImuData> imuSet;
        int collectRes = CollectImuByTimeSpan(imu_data, imuSet, time_start, time_end);
        const auto &time_span = common::ToSeconds(time_end - time_start);
        LOG(INFO) << "collect imu res: " << collectRes << " org_size " << imu_data.size() << " time_span " << time_span << "\n";
        if (collectRes < 1)
            return -1;
        int data_size = imuSet.size();
        LOG(INFO) << "ImuPreintegrate: get_imu_size " << data_size << "\n";
        if (data_size < 2)
            return -2;
        ImuMiddleCalculator calc(time_start, time_end, data_size);
        // data_size - 1 very important
        for (int i = 0; i < (data_size - 1); i++)
        {
            ImuDataAlias imuCurr = imuSet[i];
            ImuDataAlias imuNext = imuSet[i + 1];
            if (calc.Calculate(imuCurr, imuNext, i) < 1)
                continue;
            integrator->Update(calc.acc.cast<float>(), calc.angular.cast<float>(), calc.tstep);
        }
        return 1;
    }

    const float eps = 1e-4;

    ImuCalib GetImuCalib()
    {
        Eigen::Matrix<double, 3, 3> rotation;
        rotation << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
        Eigen::Quaternionf quaternion(rotation.cast<float>());

        Eigen::Matrix<double, 3, 1> translation;
        translation << 0.0, 0.0, 0.0;
        Sophus::SE3f tf_imu_to_tracking = Sophus::SE3<float>(quaternion, translation.cast<float>());

        const auto &options = cartographer::ImuOptions::GetInstance();

        // double mImuPer = 1.0 / (double)options.frequency;
        // mImuPer = 0.001; //1.0 / (double) mImuFreq;
        float Ng = options.noise_gyro; // 陀螺仪噪声
        float Na = options.noise_acc;  // 加速度噪声
        float Ngw = options.walk_gyro; // 陀螺仪随机游走噪声
        float Naw = options.walk_acc;  // 加速度计随机游走噪声
        const float sf = std::sqrt(options.frequency);
        ImuCalib calib(tf_imu_to_tracking, Ng * sf, Na * sf, Ngw / sf, Naw / sf);
        return calib;
    }

    IntegratedRotation::IntegratedRotation(const Eigen::Vector3f &angVel, const ImuBias &imuBias, const float &time)
    {
        const float x = (angVel(0) - imuBias.bwx) * time;
        const float y = (angVel(1) - imuBias.bwy) * time;
        const float z = (angVel(2) - imuBias.bwz) * time;

        const float d2 = x * x + y * y + z * z;
        const float d = sqrt(d2);

        Eigen::Vector3f v;
        v << x, y, z;
        Eigen::Matrix3f W = Sophus::SO3f::hat(v);
        if (d < eps)
        {
            deltaR = Eigen::Matrix3f::Identity() + W;
            rightJ = Eigen::Matrix3f::Identity();
        }
        else
        {
            deltaR = Eigen::Matrix3f::Identity() + W * sin(d) / d + W * W * (1.0f - cos(d)) / d2;
            rightJ = Eigen::Matrix3f::Identity() - W * (1.0f - cos(d)) / d2 + W * W * (d - sin(d)) / (d2 * d);
        }
    }

    ImuPreintegrate::ImuPreintegrate(const ImuBias &b_)
    {
        const auto &calib = GetImuCalib();
        Nga = calib.Cov;
        NgaWalk = calib.CovWalk;
        Initialize(b_);
    }

    void ImuPreintegrate::Initialize(const ImuBias &b_)
    {
        dR.setIdentity();
        dV.setZero();
        dP.setZero();
        // 偏执的雅可比
        JRg.setZero();
        JVg.setZero();
        JVa.setZero();
        JPg.setZero();
        JPa.setZero();
        C.setZero();
        Info.setZero();
        db.setZero();
        b = b_;
        bu = b_;
        avgA.setZero();
        avgW.setZero();
        dT = 0.0f;
        mvMeasurements.clear();
    }

    // void ImuPreintegrate::Reintegrate()
    // {
    // 	std::unique_lock<std::mutex> lock(mMutex);
    // 	const std::vector<integrable> aux = mvMeasurements;
    // 	Initialize(bu);
    // 	for (size_t i = 0; i < aux.size(); i++)
    // 		Update(aux[i].a, aux[i].w, aux[i].t);
    // }

    /*
    https://zhuanlan.zhihu.com/p/511529274
    http://www.shouxieziti.cn/113163.html
    https://blog.csdn.net/qq_39266065/article/details/115348824
    积分项是相对于第i时刻的姿态，而不是相对于世界坐标系的姿态
    */
    void ImuPreintegrate::Update(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt)
    {
        // 将所有的数据保存起来
        mvMeasurements.push_back(integrable(acceleration, angVel, dt));
        /*
        基于误差随时间变化的递推方程，其中误差指的是
        误差包含两部分:1) 当前预积分量误差传递给下一时刻 2) 当前时刻测量噪声传递给下一时刻
        误差随时间变化的导数关系: δx˙= Aδx + Bn
        误差状态的传递方程: δx(k) = δx(k−1) +δx˙(k−1)∆t 即 δx(k) = (I + A∆t)δx(k−1) + B∆tn(k−1)
        */
        // A为预积分量误差的传递矩阵，预积分量误差项包括：旋转，速度，位移, 每项xyz三个维度, 故共9维。
        Eigen::Matrix<float, 9, 9> A;
        A.setIdentity();
        // B为测量噪声的传递矩阵
        Eigen::Matrix<float, 9, 6> B;
        B.setZero();
        // 这两个传递矩阵的数都是当前时刻的，用其计算下一时刻的矩阵

        Eigen::Vector3f acc, accW;
        // 减去bias
        acc << acceleration(0) - b.bax, acceleration(1) - b.bay, acceleration(2) - b.baz;
        accW << angVel(0) - b.bwx, angVel(1) - b.bwy, angVel(2) - b.bwz;
        // 计算加速度和陀螺仪的历史平均值，对计算预积分暂时无用
        // dT是本次积分的累计时间，dt是两个IMU数据的时间间隔
        // 平均加速度 = (累计时间*上时刻平均加速 + 预积分旋转量*当前时刻IMU加速度*两相邻IMU时间差)
        // 逻辑是:认为在累计时间dT内匀加速，然后再加上一个速度增量，再除以总的时间，得到匀加速度
        avgA = (dT * avgA + dR * acc * dt) / (dT + dt);
        avgW = (dT * avgW + accW * dt) / (dT + dt);

        // Update delta position dP and velocity dV (rely on no-updated delta rotation)
        // 基于还未更新的上一时刻的旋转更新相邻时刻IMU的累计位移和速度
        // 更新预积分量中的位置项
        dP = dP + dV * dt + 0.5f * dR * acc * dt * dt;
        // 更新预积分量中的速度项
        dV = dV + dR * acc * dt;

        // Compute velocity and position parts of matrices A and B (rely on non-updated delta rotation)
        // 将加速度向量转为反对称矩阵
        Eigen::Matrix<float, 3, 3> Wacc = Sophus::SO3f::hat(acc);

        A.block<3, 3>(3, 0) = -dR * dt * Wacc;
        A.block<3, 3>(6, 0) = -0.5f * dR * dt * dt * Wacc;
        A.block<3, 3>(6, 3) = Eigen::DiagonalMatrix<float, 3>(dt, dt, dt);
        B.block<3, 3>(3, 3) = dR * dt;
        B.block<3, 3>(6, 3) = 0.5f * dR * dt * dt;

        // Update position and velocity jacobians wrt bias correction
        // 因为随着时间推移，不可能每次都重新计算雅克比矩阵，所以需要做J(k+1) = j(k) + (~)这类事，分解方式与AB矩阵相同
        // 论文作者对forster论文公式的基础上做了变形，然后递归更新。
        // 参见 https://github.com/UZ-SLAMLab/ORB_SLAM3/issues/212

        // 位移与加速偏置的雅可比
        JPa = JPa + JVa * dt - 0.5f * dR * dt * dt;
        JPg = JPg + JVg * dt - 0.5f * dR * dt * dt * Wacc * JRg;
        JVa = JVa - dR * dt;
        JVg = JVg - dR * dt * Wacc * JRg;

        // Update delta rotation
        IntegratedRotation dRi(angVel, b, dt);
        // 更新旋转
        dR = NormalizeRotation<float>(dR * dRi.deltaR);

        // Compute rotation parts of matrices A and B
        A.block<3, 3>(0, 0) = dRi.deltaR.transpose();
        B.block<3, 3>(0, 0) = dRi.rightJ * dt;

        // Update covariance
        C.block<9, 9>(0, 0) = A * C.block<9, 9>(0, 0) * A.transpose() + B * Nga * B.transpose();
        C.block<6, 6>(9, 9) += NgaWalk;
        // Update rotation jacobian wrt bias correction
        JRg = dRi.deltaR.transpose() * JRg - dRi.rightJ * dt;
        // Total integrated time
        dT += dt;
    }

    void ImuPreintegrate::SetNewBias(const ImuBias &bu_)
    {
        std::unique_lock<std::mutex> lock(mMutex);
        bu = bu_;
        db(0) = bu_.bwx - b.bwx;
        db(1) = bu_.bwy - b.bwy;
        db(2) = bu_.bwz - b.bwz;
        db(3) = bu_.bax - b.bax;
        db(4) = bu_.bay - b.bay;
        db(5) = bu_.baz - b.baz;
    }

    ImuBias ImuPreintegrate::GetDeltaBias(const ImuBias &b_)
    {
        std::unique_lock<std::mutex> lock(mMutex);
        return ImuBias(b_.bax - b.bax, b_.bay - b.bay, b_.baz - b.baz, b_.bwx - b.bwx, b_.bwy - b.bwy, b_.bwz - b.bwz);
    }

    Eigen::Matrix3f ImuPreintegrate::GetDeltaRotation(const ImuBias &b_)
    {
        std::unique_lock<std::mutex> lock(mMutex);
        Eigen::Vector3f dbg;
        dbg << b_.bwx - b.bwx, b_.bwy - b.bwy, b_.bwz - b.bwz;
        return NormalizeRotation<float>(dR * Sophus::SO3f::exp(JRg * dbg).matrix());
    }

    Eigen::Vector3f ImuPreintegrate::GetDeltaVelocity(const ImuBias &b_)
    {
        std::unique_lock<std::mutex> lock(mMutex);
        Eigen::Vector3f dbg, dba;
        dbg << b_.bwx - b.bwx, b_.bwy - b.bwy, b_.bwz - b.bwz;
        dba << b_.bax - b.bax, b_.bay - b.bay, b_.baz - b.baz;
        return dV + JVg * dbg + JVa * dba;
    }

    Eigen::Vector3f ImuPreintegrate::GetDeltaPosition(const ImuBias &b_)
    {
        std::unique_lock<std::mutex> lock(mMutex);
        Eigen::Vector3f dbg, dba;
        dbg << b_.bwx - b.bwx, b_.bwy - b.bwy, b_.bwz - b.bwz;
        dba << b_.bax - b.bax, b_.bay - b.bay, b_.baz - b.baz;
        return dP + JPg * dbg + JPa * dba;
    }

    Eigen::Matrix3f ImuPreintegrate::GetUpdatedDeltaRotation()
    {
        std::unique_lock<std::mutex> lock(mMutex);
        return NormalizeRotation<float>(dR * Sophus::SO3f::exp(JRg * db.head(3)).matrix());
    }

    Eigen::Vector3f ImuPreintegrate::GetUpdatedDeltaVelocity()
    {
        std::unique_lock<std::mutex> lock(mMutex);
        return dV + JVg * db.head(3) + JVa * db.tail(3);
    }

    Eigen::Vector3f ImuPreintegrate::GetUpdatedDeltaPosition()
    {
        // 雅可比 * 加速偏置增量 = 当前偏置
        // 积分位移 + 当前偏置 = 得到真实位移
        std::unique_lock<std::mutex> lock(mMutex);
        return dP + JPg * db.head(3) + JPa * db.tail(3);
    }

    Eigen::Matrix3f ImuPreintegrate::GetOriginalDeltaRotation()
    {
        std::unique_lock<std::mutex> lock(mMutex);
        return dR;
    }

    Eigen::Vector3f ImuPreintegrate::GetOriginalDeltaVelocity()
    {
        std::unique_lock<std::mutex> lock(mMutex);
        return dV;
    }

    Eigen::Vector3f ImuPreintegrate::GetOriginalDeltaPosition()
    {
        std::unique_lock<std::mutex> lock(mMutex);
        return dP;
    }

    ImuBias ImuPreintegrate::GetOriginalBias()
    {
        std::unique_lock<std::mutex> lock(mMutex);
        return b;
    }

    ImuBias ImuPreintegrate::GetUpdatedBias()
    {
        std::unique_lock<std::mutex> lock(mMutex);
        return bu;
    }

    Eigen::Matrix<float, 6, 1> ImuPreintegrate::GetDeltaBias()
    {
        std::unique_lock<std::mutex> lock(mMutex);
        return db;
    }

    void ImuCalib::Set(const Sophus::SE3<float> &sophTbc, const float &ng, const float &na, const float &ngw, const float &naw)
    {
        mbIsSet = true;
        const float ng2 = ng * ng;
        const float na2 = na * na;
        const float ngw2 = ngw * ngw;
        const float naw2 = naw * naw;
        // Sophus/Eigen
        mTbc = sophTbc;
        mTcb = mTbc.inverse();
        Cov.diagonal() << ng2, ng2, ng2, na2, na2, na2;
        CovWalk.diagonal() << ngw2, ngw2, ngw2, naw2, naw2, naw2;
    }

    ImuCalib::ImuCalib(const ImuCalib &calib)
    {
        mbIsSet = calib.mbIsSet;
        // Sophus/Eigen parameters
        mTbc = calib.mTbc;
        mTcb = calib.mTcb;
        Cov = calib.Cov;
        CovWalk = calib.CovWalk;
    }
} // namespace cartographer